#include <Wire.h>

/* 
Lite Brite 1.1
 
 */

#include "BlinkM_funcs.h"

#define blinkm_addr 0x00

const int potPin = 1;
// these constants won't change:
const int ledPin = 3;      // led connected to digital pin 13
//const int ledDetectPin = 12; 
const int redPin = 9; 
const int greenPin = redPin+1; 
const int bluePin = greenPin+1; 
const int whitePin = 6;

const int knockSensor = 0;  // the piezo is connected to analog pin 0
const int slop = 6;
const int bias_level = 263;
const int threshold = bias_level + slop;  // threshold value to decide when the detected sound is a knock or not
int calibrated_count = 0;
const int sample_len = 70;
const int transient_factor = 10; //lower numbers mean a

int averages[100];
int sampleCount=0;

int maxSensorRead = 0;


//bpm-related stuff
unsigned long millisSinceStart = 0; //millis()
unsigned long millisAtStart = 0;
unsigned long timeOfLastDetect = 0; //millis()
boolean prevDetectState = false;
unsigned long numBeats = 0;
float systemBPM = 0;
int bpmResetRateMS = 10 * 1000; 

// these variables will change:
int sensorReading = 0;      // variable to store the value read from the sensor pin
int ledState = LOW;         // variable used to store the last LED status, to toggle the light



int getTriggerLevel () {
  //return (int)round( getSensitiviyFactor() ) * 6;  //scale trigger based on potentiometer value
  return getMaxSensorRead() * 0.8 ; //as a percentage of the maximum
}

int getPotValue (int analogPinNum) {
  return analogRead(analogPinNum); 
}


float getNormalizedPotValue (int analogPinNum, float normalizeFactor) {
  float val = 0;
  val = getPotValue(analogPinNum) * normalizeFactor;
  return val;
}


float getSensitiviyFactor() {
  float myVal = getNormalizedPotValue( potPin, 0.004 );
  return myVal;
}


// ######################################################################
// T. Nathan Mundhenk
// mundhenk@usc.edu
// C/C++ Macro HSV to RGB
//#define PIX_HSV_TO_RGB_COMMON(H,S,V,R,G,B)  
//ote: S and V are normalized between 0 and 1 in this example. H is its normal 0 to 360. 
void hsvToRGB (const float H, const float S, const float V, float& R, float& G, float& B) {
  if( V == 0 )                                                        
  { 
    R = 0;     G = 0;     B = 0; 
  }                                            
  else if( S == 0 )                                                   
  {                                                                   
    R = V;       G = V;      B = V;                                                            
  }                                                                   
  else                                                                
  {                                                                   
    const double hf = float(H / 60.0);                                       
    const int    i  = (int) floor( hf );  
    const double f  = hf - i;                                         
    const double pv  = V * ( 1 - S )  ; 
    const double qv  = (float)V * (float)( 1.0 - (float)S * f );                             
    const double tv  = (float)V * (float)( 1.0 - S * ( 1.0 - (float)f ) );                     
    switch( i )                                                       
    {                                                               
      //Red is the dominant color
    case 0:                                                         
      R = V;    G = tv;      B = pv;       break;                                                        
      //Green is the dominant color
    case 1:                                                         
      R = qv;       G = V;       B = pv;       break;                                                        
    case 2:                                                         
      R = pv;        G = V;         B = tv;        break;                                                        
      //Blue is the dominant color
    case 3:                                                         
      R = pv;        G = qv;         B = V;        break;                                                        
    case 4:                                                         
      R = tv;       G = pv;        B = V;        break;                                                        
      //Red is the dominant color
    case 5:                                                         
      R = V;              G = pv;         B = qv;      break;                                                        
      //Just in case we overshoot on our math by a little, we put these here. Since its a switch it won't slow us down at all to put these here.
      case 6:                                                         
      R = V;                                                        
      G = tv;       B = pv;           break;                                                        
    case -1:                                                        
      R = V;                                                        
      G = pv;             B = qv;           break;                                                        
      //The color is not defined, we should throw an error.
    default:                                                        
      Serial.println("i Value error in Pixel conversion, Value is ");   
      break;                                                        
    }    
    R *= 255.0F;    
    G *= 255.0F;                                                        
    B *= 255.0F;
  }              
}



void setup() {
  pinMode(ledPin, OUTPUT); // declare the ledPin as as OUTPUT
  pinMode(redPin, OUTPUT); // declare the ledPin as as OUTPUT
  pinMode(greenPin, OUTPUT); // declare the ledPin as as OUTPUT
  pinMode(bluePin, OUTPUT); // declare the ledPin as as OUTPUT
  pinMode(whitePin, OUTPUT); // declare the ledPin as as OUTPUT
    //Serial.begin(9600);       // use the serial port
  Serial.begin(19200);       // use the serial port
  for (int i=0; i<sample_len; i++) {
    averages[i]=200;
  }
  setMaxSensorRead(10); //start off low
  resetBPM();
  millisAtStart = millis();
  initBlinkM();
}

float rolling_average (int newReading) {
  long int avg = 0;
  int myMin =16000;
  int myMax = 0;

  //add the new line to the array
  if (sampleCount > sample_len) {
    sampleCount=0;
  }
  averages[sampleCount]=newReading;
  sampleCount++;

  //sum the array of samples
  for (int i=0; i<sample_len; i++) {
    avg = avg + averages[i];
  }
  float fl_avg = (float)avg / (float)(sample_len);
  return fl_avg;
}



int getRMS (int newReading) {
  double avg = 0.0;
  //add the new line to the array
  if (sampleCount >=sample_len) {
    sampleCount=0;
  }
  averages[sampleCount]=newReading;
  sampleCount++;
  //sum the array of samples
  for (int i=0; i<sample_len; i++) {
    avg = avg + (double)averages[i] * (double)averages[i];
  }
  double fl_avg = double(sqrt(avg));
  return (int) round(fl_avg);
}


int getMaxSensorRead () {
  int val = maxSensorRead;

  return val;
}

void setMaxSensorRead(int counts) {
  if (counts > maxSensorRead) {
    maxSensorRead = counts;
  }
}

void overrideMaxSensorRead(int counts) { 
   if (counts < 60) {
     //don't go too low, or it will trigger all the time
    return;
  }
    maxSensorRead = counts;
}


void initBlinkM() {
  BlinkM_beginWithPower();
  BlinkM_stopScript(blinkm_addr);  // turn off startup script
}


void setRGBColor(int myR,  int myG, int myB, int myW) {
  // update the LED pin itself:        
  analogWrite(redPin, (byte)myR);     
  analogWrite(greenPin, (byte)myG);
  analogWrite(bluePin, (byte)myB);
  analogWrite(whitePin, (byte)myW);
}

void setColor (int wowFactor) {
  float bpm = getBPM();
  float quarterNoteMS = 0;
  float phase = 0.0;
  float timeMS = 0;
  quarterNoteMS = 60000.0 / bpm; 
  float halfNoteMS = quarterNoteMS *2.0;
  float wholeNoteMS =  quarterNoteMS * 4.0;
  float sixteenthNoteMS = quarterNoteMS / 2.0;
  timeMS = (float)millis() - (float)timeOfLastDetect;
   // set blinkms with hue & bri, saturation is max
  //BlinkM_fadeToHSB( blinkm_addr, colorSweep, blue, myBrightness );
  //BlinkM_setRGB(blinkm_addr,  red,  green,  blue);
  float myR = 0.0;
  float myG = 0.0;
  float myB = 0.0;
//H is hue
  float H = sin( (  (float) ( (float)timeMS/(float)quarterNoteMS ) + (phase * 1) )*PI ) + 1 ;
  H = (float)H * 180.0 ;
  //S is saturation
  float S = sin( (  (float) ( (float)timeMS/(float)halfNoteMS )  )*PI ) + 1 ;
  S =  (S / 4.0) + 0.5; //range should be 0.5 to 1.0
  
  //V is brightness
  float V  = 0;
   Serial.print("Wow factor: ");
   Serial.print(wowFactor);
   Serial.print(", trigger=");
   Serial.print(getTriggerLevel());
    Serial.print(", ms since start=");
      Serial.println(millisSinceStart);
   
         
  if (abs(wowFactor) <  getTriggerLevel() ) {
    //use sine wave for brightness
    V = cos( (  (float) ( (float)timeMS/(float)quarterNoteMS )  )*PI ) + 1 ;
    V = V / 2.0;
  } 
  else {
    //use the actual reading we're getting off the sensor
     Serial.print("==Trigger!");
    V  = map(abs(wowFactor), 0, getMaxSensorRead(), 0, 255);
  }

  int white = map(abs(wowFactor), 0, getMaxSensorRead(), 0, 255);
  hsvToRGB(H, S, V, myR, myG, myB);
  BlinkM_setRGB(blinkm_addr,  (byte)round(myR),  (byte)round(myG),  (byte)round(myB));
  setRGBColor((byte)round(myR),  (byte)round(myG),  (byte)round(myB), (byte)round(white));
}




void calcBPM() {
  float bpmAvg = 60000 * numBeats  / (millis() - millisSinceStart);
  if (bpmAvg <= 0.0) bpmAvg = 4;
  systemBPM = ( bpmAvg + systemBPM ) /2.0 ;  // smooth out a little with an average
  //Serial.print("bpm: ");
  //Serial.println(systemBPM);
}


float getBPM() {
     calcBPM();
  if ( millis() - timeOfLastDetect >= bpmResetRateMS / 2 ) {
    //want to reset, because we're past half the interval, and we have *not* seen any beats
    resetBPM();
    int val = getMaxSensorRead();
    overrideMaxSensorRead(val / 2); //start off low
  }

 
  //keep the range within something reasonable
   if (systemBPM <= 0.0) systemBPM = 4;
   if (systemBPM > 144.0) systemBPM = 144;
  return systemBPM;
}

void resetBPM() {
  Serial.println("===========================RESET========================");
  
  millisSinceStart = millis();
  //timeOfLastDetect = millisSinceStart;
  numBeats = 0;
}

void processNoDetection() {
  //no detection, give up
  digitalWrite(ledPin, 0);
  prevDetectState = false;
}

void processDetection(int calibrated_count) {
  ledState = map(abs(calibrated_count), 0, getMaxSensorRead(), 0, 255);
  // update the LED pin itself:        
  digitalWrite(ledPin, ledState);
  //analogWrite(ledPin, ledState);
  numBeats++;
  prevDetectState = true;
  timeOfLastDetect = millis();
  calcBPM();
}

void detectBeat (int calibrated_count, int allowable_variance) {
  int myColor = map(abs(calibrated_count), 0, getMaxSensorRead(), 0, 255);
  setColor(myColor);

  //if there's nothing to do, bail
  if (calibrated_count  <= allowable_variance  ) {
    processNoDetection();
    return;
  }

  //if we detected in the last sample, don't do it again
  if (prevDetectState == true) {
    processNoDetection();
    return;
  }

  //if it's not been enough time, bail
  float bpm = 128;
  float quarterNoteMS = 0;
  unsigned long timeMS = 0;
  quarterNoteMS = 60000.0 / bpm; 
  int dottedeighthNoteMS = (int)round(quarterNoteMS / 1.5);
  timeMS = millis();
  if (  timeMS - timeOfLastDetect <= dottedeighthNoteMS  )  {
    processNoDetection();
    return;
  }

  Serial.println("Beat!");
  processDetection(calibrated_count);
  return;
}



void loop() {
  // read the sensor and store it in the variable sensorReading:
  sensorReading = analogRead(knockSensor);  
  float myAverage = rolling_average(sensorReading);
  int allowable_variance = abs( floor( sqrt(myAverage) * getSensitiviyFactor() ) );
  //add a small # of counts as an error gaurd
  if (allowable_variance <= 0) allowable_variance = allowable_variance + 1;
  calibrated_count = abs (sensorReading - ( (int)myAverage  ) );
  setMaxSensorRead(calibrated_count);
  detectBeat(calibrated_count, allowable_variance);
  //Serial.print(sensorReading);
  
  /*
  //   Serial.print(" AVG:");
  Serial.print(",");
  Serial.print(myAverage);
  // Serial.print("   CAL:");
  Serial.print(",");
  Serial.print(calibrated_count);
  // Serial.print(" VAR:");
  Serial.print(",");
  Serial.println(allowable_variance );
  //Serial.println(",");
*/
  delay(1);  // delay to avoid overloading the serial port buffer
}


